#include "WPILib.h"
#include "Constants.h"
#include "input.h"

InputHandler::InputHandler() {
	m_ds = DriverStation::GetInstance();
	
	m_rightStick = new Joystick(RIGHT_JOYSTICK);
	m_leftStick = new Joystick(LEFT_JOYSTICK);
	m_steeringWheel = new Joystick(STEERING_WHEEL);
	
	// Iterate over all the buttons on each joystick, setting state to false for each
	UINT8 buttonNum = 1;						// start counting buttons at button 1
	for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
		m_rightStickButtonState[buttonNum] = false;
		m_leftStickButtonState[buttonNum] = false;
	}
	
	m_frontSteering = new AnalogChannel(FRONT_POT);
	m_backSteering = new AnalogChannel(BACK_POT);
	m_turretSteering = new AnalogChannel(TURRET_POT);
	
	m_frontBanner = new DigitalInput(1);
	m_alliance = new DigitalInput(3);
	
	m_auton0 = new DigitalInput(4);
	m_auton1 = new DigitalInput(5);
}
	
void InputHandler::Run() {
	HandleJoystick(m_rightStick, m_rightStickButtonState);
	HandleJoystick(m_leftStick, m_leftStickButtonState);
}

void InputHandler::HandleJoystick(Joystick *currStick, bool *buttonPreviouslyPressed) {
	for (UINT8 buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
				if (currStick->GetRawButton(buttonNum))
					buttonPreviouslyPressed[buttonNum] = true;
				else
					buttonPreviouslyPressed[buttonNum] = false;
			}
}

float InputHandler::GetRightJoy()  {
	return m_rightStick->GetRawAxis(2);
}

float InputHandler::GetLeftJoy() {
	return m_leftStick->GetRawAxis(2);
}

bool InputHandler::GetTargetOrientation() {
	return m_alliance->Get();
}

float InputHandler::GetDriveSpeed() {
	return m_leftStick->GetRawAxis(2);
}

float InputHandler::GetWheelValue() {
	return m_steeringWheel->GetAxis(m_steeringWheel->kXAxis);
}

float InputHandler::GetFrontAngle() {
	// right bound .084 left bound -.14
	float out = ((float)(m_frontSteering->GetValue() - 512) /512.0);
	if (out < -.14) return -2;
	else if (out > .084) return 2;
	else if (out > 0) return out / .084;
	else if (out < 0) return out / .14;
	else return 0;
	//return ((float)(m_frontSteering->GetValue() - 512) /512.0);
}

float InputHandler::GetBackAngle() {
	// right bound .22 left bound -.15
	float out = ((float)(m_backSteering->GetValue() - 512) /512.0);
	if (out < -.18) return -2;
	else if (out > .09) return 2;
	else if (out > 0) return out / .09;//.075
	else if (out < 0) return out / .18;
	else return 0;
	//return ((float)(m_backSteering->GetValue() - 512) /512.0);
}

float InputHandler::GetTurretAngle() {
	float out = ((float)(m_turretSteering->GetValue() - 512) /512.0);
	//out = out - .4;
	//if (out < -1) out = 1 - (-out - 1);
	//if (out < -.3) return -2;
	//else if (out > .3) return 2;
	//printf("TURRET:%f\n", out);
	return out;
}

bool InputHandler::GetFrontBanner()  {
	return m_frontBanner->Get();
}

bool InputHandler::GetPureAutonomous()  {
	return m_ds->GetDigitalIn(1);
}

bool InputHandler::GetAutoTrack() {
	return m_ds->GetDigitalIn(2);
}

bool InputHandler::GetAutoFire() {
	return m_ds->GetDigitalIn(4);
}

bool InputHandler::GetGyro() {
	return m_ds->GetDigitalIn(3);
}

float InputHandler::GetAimTurret() {
	return ((float)(m_ds->GetAnalogIn(1) - 512) /512.0);
}

bool InputHandler::GetFire() {
	return m_ds->GetDigitalIn(6);
}

int InputHandler::GetMoveBelts() {
	if (m_ds->GetDigitalIn(5)) {
		return 1;
	} else if (m_ds->GetDigitalIn(7)) {
		return -1;
	} else {
		return 0;
	}
}

bool InputHandler::HasBalls() {
	return true;
}

int InputHandler::GetAutonMode()  {
	int mode = 0;
	if (m_auton0->Get() == 1) {mode += 1;}
	if (m_auton1->Get() == 1) {mode += 2;}
	return mode;
}
